#include "stdafx.h"
#include <stdio.h>
#include <iostream>

#include "RAPID/RAPID.H"

// classes:
#include "Robot.h"

#include "StaticObject.h"
#include "CollisionDetection.h"


int t=0;
int tm=0;


/// @todo kolekcja wskaznikow do obiektow zamiast deklarowanych statycznie
// globalne deklaracje modulow lub calych robotow
Robot * kawasaki;
Robot * mitsubishi;
StaticObject * tasma;

/**
 * Inicjalizacja modeli robotow
 * @return zawsze 0
 */
int InitializeModels(void)
{

    kawasaki=new Robot("kawasaki");
    kawasaki->translateModulesToBase();       // przeniesienie wszystkich modulow do punktu (0,0,0)
    kawasaki->transform(0);     // przesuniecie robota w zadanym kierunku

//
    mitsubishi=new Robot("mitsubishi");
    mitsubishi->transform(0);	

    #ifdef TASMA
    tasma = new StaticObject("tasma");
    #endif

	return 0;

}

/**
 * Aktualizacja modeli robotow.
 * Modele sa aktualizowane na podstawie odebranego bufora polozen.
 * Ponadto wystepuje ty wykrywanie kolizji robotow.
 * @todo przeniesc wykrywanie kolizji do oddzielnego wywolania
 */
void ComputePositions(void)
{
		
		kawasaki->updatePositions();
		mitsubishi->updatePositions();

        if (t % 10 == 0)
        {
            if (CollisionDetection::FindFirstCollisionInAllObjects(0))
            {
                ;
            //    printf("STOP!");
            //    fflush (stdout);
            }						
        }

        t++;        
}
